#include <Servo.h>
#include <Wire.h>
#include <U8g2lib.h>
#include <SPI.h>
// 使用旋转180度的配置 (U8G2_R2)
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R2, /* reset=*/ U8X8_PIN_NONE);

// 命令定义
#define CMD_IDLE        0x00
#define CMD_STAND       0x01
#define CMD_WALK_FWD    0x02
#define CMD_WALK_BWD    0x03
#define CMD_RUN_BWD     0x04
#define CMD_TURN_L      0x05
#define CMD_TURN_R      0x06
#define CMD_LIE         0x07
#define CMD_SIT         0x08
#define CMD_STATUS      0x55

volatile byte currentCommand = CMD_IDLE;  // 当前执行的命令
volatile byte receivedData = 0;
volatile bool dataReceived = false;

// 表情类型枚举
enum Emotion {
  NEUTRAL,
  HAPPY,
  EXCITED,
  SLEEPY,
  CONFUSED,
  SURPRISED,
};

// 创建舵机对象
Servo servo_D3;   // 左前腿
Servo servo_D5;   // 右前腿
Servo servo_D6;   // 左后腿
Servo servo_D9;   // 右后腿

// 定义舵机引脚
const int servoPin_D3 = 3;
const int servoPin_D5 = 5;
const int servoPin_D6 = 6;
const int servoPin_D9 = 9;

// 初始位置
const int INIT_D3 = 90;
const int INIT_D5 = 90;
const int INIT_D6 = 100;
const int INIT_D9 = 80;

// 前进参数 (标准前进)
int FWD_LIFT_HEIGHT = 25;      // 抬腿高度
int FWD_STRIDE_LENGTH = 50;    // 步幅长度
int FWD_BODY_DROP = 40;        // 身体下压量
int FWD_STEP_DELAY = 60;       // 步延迟
int MOVE_SPEED = 15;
int WALK_CYCLES = 6;           // 每阶段细分步数

// 后退参数 (标准后退)
int BWD_LIFT_HEIGHT = 25;      // 抬腿高度 (与前进相同)
int BWD_STRIDE_LENGTH = 40;    // 步幅长度 (比前进小)
int BWD_BODY_DROP = 15;        // 身体下压量 (比前进小)
int BWD_STEP_DELAY = 60;       // 步延迟 (与前进相同)

// 转弯参数
float TURN_FACTOR_LEFT = 0;  // 左转时内侧腿步幅系数 (0.0~1.0)
float TURN_FACTOR_RIGHT = 0; // 右转时内侧腿步幅系数 (0.0~1.0)
const int DEFAULT_TURN_STEPS = 9;  // 默认转弯步数

// 后退奔跑参数 (独立参数)
int RUN_BWD_LIFT_HEIGHT = 30;    // 后退奔跑抬腿高度
int RUN_BWD_STRIDE_LENGTH = 55;  // 后退奔跑步幅长度
int RUN_BWD_BODY_DROP = 30;      // 后退奔跑身体下压量
int RUN_BWD_STEP_DELAY = 45;     // 后退奔跑步延迟

// 行走方向
int walkDirection = 1;   // 1=前进, -1=后退

// 当前表情
Emotion currentEmotion = NEUTRAL;
unsigned long lastBlinkTime = 0;
bool eyesClosed = false;

void setup() {
  pinMode(MISO, OUTPUT);  // 将MISO设为输出（从设备输出模式）
  SPCR |= _BV(SPE);      // 启用SPI从模式
  SPCR |= _BV(SPIE);     // 启用中断
  Serial.begin(9600);
  Serial.println(F("四足机器人控制器就绪"));
  Serial.println(F("命令: F(前进), B(后退), R(后退奔跑), A(左转), D(右转), S(站立), T(坐下), L(趴下), C(配置)"));
  
  
  // OLED初始化
  u8g2.begin();
  u8g2.setFontMode(1); // 启用透明字体模式
  u8g2.setFontDirection(0); // 正常方向
  
  // 显示启动画面
  displayEmotion(NEUTRAL);
  
  // 绑定舵机并初始化位置
  servo_D3.attach(servoPin_D3);
  servo_D5.attach(servoPin_D5);
  servo_D6.attach(servoPin_D6);
  servo_D9.attach(servoPin_D9);
  
  stand();  // 站立到初始位置
}

// SPI中断服务程序
ISR(SPI_STC_vect) {
  receivedData = SPDR;  // 读取接收到的数据
  dataReceived = true;
  
  // 立即处理状态查询命令
  if(receivedData == CMD_STATUS) {
    SPDR = currentCommand;  // 返回当前状态
  } 
  else {
    SPDR = receivedData;    // 回显接收的命令
  }
}


void loop() {

  // 处理串口命令
  if (Serial.available() > 0) {
    char command = Serial.read();
    processCommand(command);
  }

  if (dataReceived) {
    dataReceived = false;
    // 处理命令
    if (receivedData == 0x01) {
      char command = 'S';
      currentCommand = CMD_STAND;
      processCommand(command);
    } else if (receivedData == 0x02) {
      char command = 'F';
      processCommand(command);
      currentCommand = CMD_WALK_FWD;
    } else if (receivedData == 0x03) {
      char command = 'B';
      processCommand(command);
      currentCommand = CMD_WALK_BWD;
    } else if (receivedData == 0x04) {
      char command = 'R';
      processCommand(command);
      currentCommand = CMD_RUN_BWD;
    } else if (receivedData == 0x05) {
      char command = 'A';
      processCommand(command);
      currentCommand = CMD_TURN_L;
    } else if (receivedData == 0x06) {
      char command = 'D';
      processCommand(command);
      currentCommand = CMD_TURN_R;
    } else if (receivedData == 0x07) {
      char command = 'L';
      processCommand(command);
      currentCommand = CMD_LIE;
    } else if (receivedData == 0x08) {
      char command = 'T';
      processCommand(command);
      currentCommand = CMD_SIT;
    }
  }
  
  // 如果当前是 NEUTRAL 表情，则持续进行眨眼动画
  static unsigned long neutralBlinkInterval = 1500; // 可调整眨眼间隔
  static unsigned long lastNeutralBlinkTime = 0;
  if (currentEmotion == NEUTRAL) {
     // 每隔 neutralBlinkInterval 毫秒眨一次眼
     if (millis() - lastNeutralBlinkTime >= neutralBlinkInterval) {
         blinkAnimation(); // 这个函数内部处理了 eyesClosed 状态和重绘
         lastNeutralBlinkTime = millis();
     }
  } else {
      // 对于非 NEUTRAL 表情，保留原来的 3 秒眨眼逻辑
      if (millis() - lastBlinkTime > 3000) {
          // 检查是否在眨眼时已经切换到了 NEUTRAL
          if(currentEmotion == NEUTRAL) {
              lastBlinkTime = millis();
          } else {
              u8g2.clearBuffer(); 
              blinkAnimation();
              lastBlinkTime = millis();
          }
      }
  }
}

// 处理串口命令
void processCommand(char cmd) {
  switch(cmd) {
    case 'F': // 向前走 (标准前进)
      walkDirection = 1;  // 1表示前进
      displayEmotion(EXCITED);
      Serial.println(F("标准前进"));
      walk(8, FWD_STEP_DELAY, true);  // true表示使用前进参数
      displayEmotion(HAPPY);
      break;
      
    case 'B': // 后退
      walkDirection = -1;  // -1表示后退
      displayEmotion(EXCITED);
      Serial.println(F("标准后退"));
      walk(8, BWD_STEP_DELAY, false);  // false表示使用后退参数
      displayEmotion(HAPPY);
      break;
      
    case 'R': // 后退奔跑
      displayEmotion(EXCITED);
      Serial.println(F("后退奔跑"));
      walkRunBackward(8, RUN_BWD_STEP_DELAY);
      displayEmotion(HAPPY);
      break;
      
    case 'A': // 左转
      displayEmotion(EXCITED);
      Serial.println(F("执行左转"));
      turnLeft(DEFAULT_TURN_STEPS, FWD_STEP_DELAY, TURN_FACTOR_LEFT);
      displayEmotion(HAPPY);
      break;
      
    case 'D': // 右转
      displayEmotion(EXCITED);
      Serial.println(F("执行右转"));
      turnRight(DEFAULT_TURN_STEPS, FWD_STEP_DELAY, TURN_FACTOR_RIGHT);
      displayEmotion(HAPPY);
      break;
      
    case 'S': // 站立
      displayEmotion(HAPPY);
      stand();
      break;
      
    case 'T': // 坐下
      displayEmotion(SLEEPY);
      sit();
      break;
      
    case 'L': // 趴下
      displayEmotion(SLEEPY);
      lieDown();
      break;
      
    case 'C': // 配置参数
      displayEmotion(CONFUSED);
      configureParameters();
      displayEmotion(HAPPY);
      break;
      
    case 'H': // 帮助
      printHelp();
      break;
      
    default:
      displayEmotion(CONFUSED);
      Serial.println(F("未知命令"));
      printHelp();
      delay(1000);
      displayEmotion(NEUTRAL);
  }
}

// 打印帮助信息
void printHelp() {
  Serial.println(F("可用命令:"));
  Serial.println(F("F - 标准前进8步"));
  Serial.println(F("B - 标准后退8步"));
  Serial.println(F("R - 后退奔跑8步"));
  Serial.println(F("A - 左转9步"));
  Serial.println(F("D - 右转9步"));
  Serial.println(F("S - 站立"));
  Serial.println(F("T - 坐下"));
  Serial.println(F("L - 趴下"));
  Serial.println(F("C - 配置参数"));
  Serial.println(F("H - 帮助"));
}

// 配置参数
void configureParameters() {
  Serial.println(F("当前参数:"));
  Serial.println(F("前进参数:"));
  Serial.print(F("抬腿高度(L): ")); Serial.println(FWD_LIFT_HEIGHT);
  Serial.print(F("步幅长度(S): ")); Serial.println(FWD_STRIDE_LENGTH);
  Serial.print(F("身体下沉(B): ")); Serial.println(FWD_BODY_DROP);
  Serial.print(F("步态延迟(D): ")); Serial.println(FWD_STEP_DELAY);
  
  Serial.println(F("后退参数:"));
  Serial.print(F("抬腿高度(LB): ")); Serial.println(BWD_LIFT_HEIGHT);
  Serial.print(F("步幅长度(SB): ")); Serial.println(BWD_STRIDE_LENGTH);
  Serial.print(F("身体下沉(BB): ")); Serial.println(BWD_BODY_DROP);
  Serial.print(F("步态延迟(DB): ")); Serial.println(BWD_STEP_DELAY);
  
  Serial.println(F("移动速度(M): ")); Serial.println(MOVE_SPEED);
  Serial.print(F("转弯系数(TL/TR): ")); 
  Serial.print(TURN_FACTOR_LEFT, 2); Serial.print(F("/")); 
  Serial.println(TURN_FACTOR_RIGHT, 2);
  
  Serial.println(F("后退奔跑参数:"));
  Serial.print(F("奔跑抬腿高度(RL): ")); Serial.println(RUN_BWD_LIFT_HEIGHT);
  Serial.print(F("奔跑步幅长度(RS): ")); Serial.println(RUN_BWD_STRIDE_LENGTH);
  Serial.print(F("奔跑身体下沉(RB): ")); Serial.println(RUN_BWD_BODY_DROP);
  Serial.print(F("奔跑步态延迟(RD): ")); Serial.println(RUN_BWD_STEP_DELAY);
  
  Serial.println(F("输入新值: L值,S值,B值,D值,ML值,MS值,MB值,DB值,M值,TL值,TR值,RL值,RS值,RB值,RD值 (如 'L20')"));
  Serial.println(F("输入 'X' 完成配置"));
  
  while(true) {
    if(Serial.available() > 0) {
      char param = Serial.read();
      
      if(param == 'X') {
        Serial.println(F("配置已保存"));
        return;
      }
      
      if(Serial.available() >= 1) {
        int value = Serial.parseInt();
        
        switch(param) {
          case 'L':
            FWD_LIFT_HEIGHT = constrain(value, 15, 40);
            Serial.print(F("前进抬腿高度设为: ")); Serial.println(FWD_LIFT_HEIGHT);
            break;
            
          case 'S':
            FWD_STRIDE_LENGTH = constrain(value, 25, 50);
            Serial.print(F("前进步幅长度设为: ")); Serial.println(FWD_STRIDE_LENGTH);
            break;
            
          case 'B':
            FWD_BODY_DROP = constrain(value, 10, 50);
            Serial.print(F("前进身体下沉设为: ")); Serial.println(FWD_BODY_DROP);
            break;
            
          case 'D':
            FWD_STEP_DELAY = constrain(value, 30, 150);
            Serial.print(F("前进步态延迟设为: ")); Serial.println(FWD_STEP_DELAY);
            break;
            
          case 'M':
            MOVE_SPEED = constrain(value, 5, 40);
            Serial.print(F("移动速度设为: ")); Serial.println(MOVE_SPEED);
            break;
            
          case 'T':
            if (value == 1) {
              // 读取TL值
              if(Serial.available() > 0) {
                float tlValue = Serial.parseFloat();
                TURN_FACTOR_LEFT = constrain(tlValue, 0.0f, 1.0f);
                Serial.print(F("左转系数设为: ")); Serial.println(TURN_FACTOR_LEFT, 2);
              }
            } else if (value == 2) {
              // 读取TR值
              if(Serial.available() > 0) {
                float trValue = Serial.parseFloat();
                TURN_FACTOR_RIGHT = constrain(trValue, 0.0f, 1.0f);
                Serial.print(F("右转系数设为: ")); Serial.println(TURN_FACTOR_RIGHT, 2);
              }
            }
            break;
            
          case 'R':
            if (value == 1) {
              // 读取奔跑抬腿高度
              if(Serial.available() > 0) {
                int rlValue = Serial.parseInt();
                RUN_BWD_LIFT_HEIGHT = constrain(rlValue, 20, 45);
                Serial.print(F("奔跑抬腿高度设为: ")); Serial.println(RUN_BWD_LIFT_HEIGHT);
              }
            } else if (value == 2) {
              // 读取奔跑步幅长度
              if(Serial.available() > 0) {
                int rsValue = Serial.parseInt();
                RUN_BWD_STRIDE_LENGTH = constrain(rsValue, 30, 70);
                Serial.print(F("奔跑步幅长度设为: ")); Serial.println(RUN_BWD_STRIDE_LENGTH);
              }
            } else if (value == 3) {
              // 读取奔跑身体下沉
              if(Serial.available() > 0) {
                int rbValue = Serial.parseInt();
                RUN_BWD_BODY_DROP = constrain(rbValue, 5, 35);
                Serial.print(F("奔跑身体下沉设为: ")); Serial.println(RUN_BWD_BODY_DROP);
              }
            } else if (value == 4) {
              // 读取奔跑步态延迟
              if(Serial.available() > 0) {
                int rdValue = Serial.parseInt();
                RUN_BWD_STEP_DELAY = constrain(rdValue, 20, 100);
                Serial.print(F("奔跑步态延迟设为: ")); Serial.println(RUN_BWD_STEP_DELAY);
              }
            }
            break;
            
          case 'l':  // 小写L表示后退参数
            if (value == 1) {
              // 读取后退抬腿高度
              if(Serial.available() > 0) {
                int lbValue = Serial.parseInt();
                BWD_LIFT_HEIGHT = constrain(lbValue, 15, 40);
                Serial.print(F("后退抬腿高度设为: ")); Serial.println(BWD_LIFT_HEIGHT);
              }
            } else if (value == 2) {
              // 读取后退步幅长度
              if(Serial.available() > 0) {
                int sbValue = Serial.parseInt();
                BWD_STRIDE_LENGTH = constrain(sbValue, 25, 50);
                Serial.print(F("后退步幅长度设为: ")); Serial.println(BWD_STRIDE_LENGTH);
              }
            } else if (value == 3) {
              // 读取后退身体下沉
              if(Serial.available() > 0) {
                int bbValue = Serial.parseInt();
                BWD_BODY_DROP = constrain(bbValue, 10, 50);
                Serial.print(F("后退身体下沉设为: ")); Serial.println(BWD_BODY_DROP);
              }
            } else if (value == 4) {
              // 读取后退步态延迟
              if(Serial.available() > 0) {
                int dbValue = Serial.parseInt();
                BWD_STEP_DELAY = constrain(dbValue, 30, 150);
                Serial.print(F("后退步态延迟设为: ")); Serial.println(BWD_STEP_DELAY);
              }
            }
            break;
            
          default:
            Serial.println(F("无效参数"));
        }
      }
    }
    delay(10);
  }
}

// 平滑移动舵机
void moveServo(Servo &servo, int target, int duration = -1) {
  if(duration < 0) duration = MOVE_SPEED;
  
  int start = servo.read();
  int steps = abs(target - start);
  if(steps == 0) return;
  
  int delayTime = duration / steps;
  delayTime = constrain(delayTime, 3, 30);
  
  int dir = (target > start) ? 1 : -1;
  
  for(int i = 0; i < steps; i++) {
    start += dir;
    servo.write(start);
    delay(delayTime);
  }
  servo.write(target); // 确保达到目标
}

// 站立姿势
void stand() {
  moveServo(servo_D3, INIT_D3);
  moveServo(servo_D5, INIT_D5);
  moveServo(servo_D6, INIT_D6);
  moveServo(servo_D9, INIT_D9);
  Serial.println(F("站立姿势完成"));
}

// 坐下姿势
void sit() {
  // 前腿保持初始位置
  moveServo(servo_D3, INIT_D3);
  moveServo(servo_D5, INIT_D5);
  
  // 后腿转到极限位置
  moveServo(servo_D6, 0, 30);    // 左后转到0°
  moveServo(servo_D9, 180, 30);  // 右后转到180°
  
  Serial.println(F("坐下姿势完成"));
}

// 趴下姿势
void lieDown() {
  // 所有腿伸展到地面位置
  moveServo(servo_D3, 0, 40);
  moveServo(servo_D5, 180, 40);
  moveServo(servo_D6, 180, 40);
  moveServo(servo_D9, 0, 40);
  Serial.println(F("趴下姿势完成"));
}

// 修正后的行走函数 - 前进和后退使用不同参数
void walk(int steps, int stepDelay, bool isForward) {
  // 选择正确的参数集
  int LIFT_HEIGHT, STRIDE_LENGTH, BODY_DROP;
  // 计算方向系数 (1=前进, -1=后退)
  int dir = walkDirection;

  if (isForward) {
    LIFT_HEIGHT = FWD_LIFT_HEIGHT;
    STRIDE_LENGTH = FWD_STRIDE_LENGTH;
    BODY_DROP = FWD_BODY_DROP;
    Serial.println(F("行走方向: 前进"));
  } else {
    LIFT_HEIGHT = BWD_LIFT_HEIGHT;
    STRIDE_LENGTH = BWD_STRIDE_LENGTH;
    BODY_DROP = BWD_BODY_DROP;
    dir = -dir;
    Serial.println(F("行走方向: 后退"));
  }
  
  // // 计算方向系数 (1=前进, -1=后退)
  // int dir = walkDirection;
  
  for(int stepCount = 0; stepCount < steps; stepCount++) {
    // 第一阶段: 抬起对角腿 (左前+右后)
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d3 = INIT_D3 - (int)(LIFT_HEIGHT * ratio);      // 左前抬腿
      int d9 = INIT_D9 + (int)(LIFT_HEIGHT * ratio);      // 右后抬腿
      int d5 = INIT_D5 + (int)(BODY_DROP * ratio);        // 右前下压
      int d6 = INIT_D6 - (int)(BODY_DROP * ratio);        // 左后下压
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
    
    // 第二阶段: 移动抬起的腿 (左前+右后)
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 使用方向系数控制摆动方向
      int d3 = INIT_D3 - LIFT_HEIGHT + (int)((STRIDE_LENGTH + LIFT_HEIGHT) * dir * ratio);
      int d9 = INIT_D9 + LIFT_HEIGHT - (int)((STRIDE_LENGTH + LIFT_HEIGHT) * dir * ratio);
      
      servo_D3.write(d3);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
    
    // 第三阶段: 放下抬起的腿 + 抬起另一对角腿
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d3 = INIT_D3 + (int)(STRIDE_LENGTH * dir) - (int)(LIFT_HEIGHT * (1.0 - ratio));
      int d9 = INIT_D9 - (int)(STRIDE_LENGTH * dir) + (int)(LIFT_HEIGHT * (1.0 - ratio));
      int d5 = INIT_D5 + (int)(BODY_DROP * (1.0 - ratio)); // 右前抬腿
      int d6 = INIT_D6 - (int)(BODY_DROP * (1.0 - ratio)); // 左后抬腿
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
    
    // 第四阶段: 抬起另一对角腿 (右前+左后)
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d5 = INIT_D5 + (int)(LIFT_HEIGHT * ratio);      // 右前抬腿
      int d6 = INIT_D6 - (int)(LIFT_HEIGHT * ratio);      // 左后抬腿
      int d3 = INIT_D3 - (int)(BODY_DROP * ratio);        // 左前下压
      int d9 = INIT_D9 + (int)(BODY_DROP * ratio);        // 右后下压
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
    
    // 第五阶段: 移动新抬起的腿 (右前+左后)
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 使用方向系数控制摆动方向
      int d5 = INIT_D5 + LIFT_HEIGHT - (int)((STRIDE_LENGTH + LIFT_HEIGHT) * dir * ratio);
      int d6 = INIT_D6 - LIFT_HEIGHT + (int)((STRIDE_LENGTH + LIFT_HEIGHT) * dir * ratio);
      
      servo_D5.write(d5);
      servo_D6.write(d6);
      
      delay(stepDelay / WALK_CYCLES);
    }
    
    // 第六阶段: 放下新抬起的腿
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d5 = INIT_D5 - (int)(STRIDE_LENGTH * dir) + (int)(LIFT_HEIGHT * (1.0 - ratio));
      int d6 = INIT_D6 + (int)(STRIDE_LENGTH * dir) - (int)(LIFT_HEIGHT * (1.0 - ratio));
      int d3 = INIT_D3 - (int)(BODY_DROP * (1.0 - ratio)); // 左前微调
      int d9 = INIT_D9 + (int)(BODY_DROP * (1.0 - ratio)); // 右后微调
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
  }
  
  // 返回站立位置
  stand();
  Serial.println(F("行走完成"));
}

// 后退奔跑函数 (使用独立参数)
void walkRunBackward(int steps, int stepDelay) {
  Serial.println(F("后退奔跑"));
  
  for(int stepCount = 0; stepCount < steps; stepCount++) {
    // 第一阶段: 抬起对角腿 (左前+右后)
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d3 = INIT_D3 - (int)(RUN_BWD_LIFT_HEIGHT * ratio);      // 左前抬腿
      int d9 = INIT_D9 + (int)(RUN_BWD_LIFT_HEIGHT * ratio);      // 右后抬腿
      int d5 = INIT_D5 + (int)(RUN_BWD_BODY_DROP * ratio);        // 右前下压
      int d6 = INIT_D6 - (int)(RUN_BWD_BODY_DROP * ratio);        // 左后下压
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
    
    // 第二阶段: 移动抬起的腿 (左前+右后)
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 后退奔跑：方向固定为-1（后退）
      int d3 = INIT_D3 - RUN_BWD_LIFT_HEIGHT + (int)((RUN_BWD_STRIDE_LENGTH + RUN_BWD_LIFT_HEIGHT) * (-1) * ratio);
      int d9 = INIT_D9 + RUN_BWD_LIFT_HEIGHT - (int)((RUN_BWD_STRIDE_LENGTH + RUN_BWD_LIFT_HEIGHT) * (-1) * ratio);
      
      servo_D3.write(d3);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
    
    // 第三阶段: 放下抬起的腿 + 抬起另一对角腿
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d3 = INIT_D3 + (int)(RUN_BWD_STRIDE_LENGTH * (-1)) - (int)(RUN_BWD_LIFT_HEIGHT * (1.0 - ratio));
      int d9 = INIT_D9 - (int)(RUN_BWD_STRIDE_LENGTH * (-1)) + (int)(RUN_BWD_LIFT_HEIGHT * (1.0 - ratio));
      int d5 = INIT_D5 + (int)(RUN_BWD_BODY_DROP * (1.0 - ratio)); // 右前抬腿
      int d6 = INIT_D6 - (int)(RUN_BWD_BODY_DROP * (1.0 - ratio)); // 左后抬腿
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
    
    // 第四阶段: 抬起另一对角腿 (右前+左后)
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d5 = INIT_D5 + (int)(RUN_BWD_LIFT_HEIGHT * ratio);      // 右前抬腿
      int d6 = INIT_D6 - (int)(RUN_BWD_LIFT_HEIGHT * ratio);      // 左后抬腿
      int d3 = INIT_D3 - (int)(RUN_BWD_BODY_DROP * ratio);        // 左前下压
      int d9 = INIT_D9 + (int)(RUN_BWD_BODY_DROP * ratio);        // 右后下压
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
    
    // 第五阶段: 移动新抬起的腿 (右前+左后)
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 后退奔跑：方向固定为-1（后退）
      int d5 = INIT_D5 + RUN_BWD_LIFT_HEIGHT - (int)((RUN_BWD_STRIDE_LENGTH + RUN_BWD_LIFT_HEIGHT) * (-1) * ratio);
      int d6 = INIT_D6 - RUN_BWD_LIFT_HEIGHT + (int)((RUN_BWD_STRIDE_LENGTH + RUN_BWD_LIFT_HEIGHT) * (-1) * ratio);
      
      servo_D5.write(d5);
      servo_D6.write(d6);
      
      delay(stepDelay / WALK_CYCLES);
    }
    
    // 第六阶段: 放下新抬起的腿
    for(int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d5 = INIT_D5 - (int)(RUN_BWD_STRIDE_LENGTH * (-1)) + (int)(RUN_BWD_LIFT_HEIGHT * (1.0 - ratio));
      int d6 = INIT_D6 + (int)(RUN_BWD_STRIDE_LENGTH * (-1)) - (int)(RUN_BWD_LIFT_HEIGHT * (1.0 - ratio));
      int d3 = INIT_D3 - (int)(RUN_BWD_BODY_DROP * (1.0 - ratio)); // 左前微调
      int d9 = INIT_D9 + (int)(RUN_BWD_BODY_DROP * (1.0 - ratio)); // 右后微调
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
  }
  
  // 返回站立位置
  stand();
  Serial.println(F("后退奔跑完成"));
}

// 左转函数 (内侧腿: 左前+左后)
void turnLeft(int steps, int stepDelay, float turnFactor) {
  for (int stepCount = 0; stepCount < steps; stepCount++) {
    Serial.print(F("  > 左转进度: "));
    Serial.print(stepCount + 1);
    Serial.print(F("/"));
    Serial.print(steps);
    Serial.print(F(" 步 | 阶段: "));
    
    // 阶段1: 抬起对角腿 (左前+右后)
    Serial.print(F("1/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d3 = INIT_D3 - (int)(FWD_LIFT_HEIGHT * ratio);      // 左前抬腿
      int d9 = INIT_D9 + (int)(FWD_LIFT_HEIGHT * ratio);      // 右后抬腿
      int d5 = INIT_D5 + (int)(FWD_BODY_DROP * ratio);        // 右前下压
      int d6 = INIT_D6 - (int)(FWD_BODY_DROP * ratio);        // 左后下压
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }

    // 阶段2: 移动抬起的腿 (左前+右后) - 左转关键: 内侧腿步幅减小
    Serial.print(F("2/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 左前(内侧)步幅 = 正常步幅 * turnFactor
      int d3 = INIT_D3 - FWD_LIFT_HEIGHT + (int)((FWD_STRIDE_LENGTH * turnFactor + FWD_LIFT_HEIGHT) * ratio);
      // 右后(外侧)保持正常步幅
      int d9 = INIT_D9 + FWD_LIFT_HEIGHT - (int)((FWD_STRIDE_LENGTH + FWD_LIFT_HEIGHT) * ratio);
      
      servo_D3.write(d3);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }

    // 阶段3: 放下抬起的腿 + 抬起另一对角腿
    Serial.print(F("3/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 左前(内侧)放腿位置 = 正常位置 + STRIDE_LENGTH * turnFactor
      int d3 = INIT_D3 + (int)(FWD_STRIDE_LENGTH * turnFactor) - (int)(FWD_LIFT_HEIGHT * (1.0 - ratio));
      // 右后(外侧)放腿位置 = 正常位置 - STRIDE_LENGTH
      int d9 = INIT_D9 - (int)(FWD_STRIDE_LENGTH) + (int)(FWD_LIFT_HEIGHT * (1.0 - ratio));
      int d5 = INIT_D5 + (int)(FWD_BODY_DROP * (1.0 - ratio)); // 右前抬腿
      int d6 = INIT_D6 - (int)(FWD_BODY_DROP * (1.0 - ratio)); // 左后抬腿
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }

    // 阶段4: 抬起另一对角腿 (右前+左后)
    Serial.print(F("4/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d5 = INIT_D5 + (int)(FWD_LIFT_HEIGHT * ratio);      // 右前抬腿
      int d6 = INIT_D6 - (int)(FWD_LIFT_HEIGHT * ratio);      // 左后抬腿
      int d3 = INIT_D3 - (int)(FWD_BODY_DROP * ratio);        // 左前下压
      int d9 = INIT_D9 + (int)(FWD_BODY_DROP * ratio);        // 右后下压
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }

    // 阶段5: 移动新抬起的腿 (右前+左后) - 左转关键: 内侧腿步幅减小
    Serial.print(F("5/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 右前(外侧)保持正常步幅
      int d5 = INIT_D5 + FWD_LIFT_HEIGHT - (int)((FWD_STRIDE_LENGTH + FWD_LIFT_HEIGHT) * ratio);
      // 左后(内侧)步幅 = 正常步幅 * turnFactor
      int d6 = INIT_D6 - FWD_LIFT_HEIGHT + (int)((FWD_STRIDE_LENGTH * turnFactor + FWD_LIFT_HEIGHT) * ratio);
      
      servo_D5.write(d5);
      servo_D6.write(d6);
      
      delay(stepDelay / WALK_CYCLES);
    }

    // 阶段6: 放下新抬起的腿
    Serial.println(F("6/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 右前(外侧)放腿位置 = 正常位置 - STRIDE_LENGTH
      int d5 = INIT_D5 - (int)(FWD_STRIDE_LENGTH) + (int)(FWD_LIFT_HEIGHT * (1.0 - ratio));
      // 左后(内侧)放腿位置 = 正常位置 + STRIDE_LENGTH * turnFactor
      int d6 = INIT_D6 + (int)(FWD_STRIDE_LENGTH * turnFactor) - (int)(FWD_LIFT_HEIGHT * (1.0 - ratio));
      int d3 = INIT_D3 - (int)(FWD_BODY_DROP * (1.0 - ratio)); // 左前微调
      int d9 = INIT_D9 + (int)(FWD_BODY_DROP * (1.0 - ratio)); // 右后微调
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
  }
  stand();
}

// 右转函数 (内侧腿: 右前+右后)
void turnRight(int steps, int stepDelay, float turnFactor) {
  for (int stepCount = 0; stepCount < steps; stepCount++) {
    Serial.print(F("  > 右转进度: "));
    Serial.print(stepCount + 1);
    Serial.print(F("/"));
    Serial.print(steps);
    Serial.print(F(" 步 | 阶段: "));
    
    // 阶段1: 抬起对角腿 (左前+右后)
    Serial.print(F("1/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d3 = INIT_D3 - (int)(FWD_LIFT_HEIGHT * ratio);      // 左前抬腿
      int d9 = INIT_D9 + (int)(FWD_LIFT_HEIGHT * ratio);      // 右后抬腿
      int d5 = INIT_D5 + (int)(FWD_BODY_DROP * ratio);        // 右前下压
      int d6 = INIT_D6 - (int)(FWD_BODY_DROP * ratio);        // 左后下压
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }

    // 阶段2: 移动抬起的腿 (左前+右后) - 右转关键: 内侧腿步幅减小
    Serial.print(F("2/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 左前(外侧)保持正常步幅
      int d3 = INIT_D3 - FWD_LIFT_HEIGHT + (int)((FWD_STRIDE_LENGTH + FWD_LIFT_HEIGHT) * ratio);
      // 右后(内侧)步幅 = 正常步幅 * turnFactor
      int d9 = INIT_D9 + FWD_LIFT_HEIGHT - (int)((FWD_STRIDE_LENGTH * turnFactor + FWD_LIFT_HEIGHT) * ratio);
      
      servo_D3.write(d3);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }

    // 阶段3: 放下抬起的腿 + 抬起另一对角腿
    Serial.print(F("3/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 左前(外侧)放腿位置 = 正常位置 + STRIDE_LENGTH
      int d3 = INIT_D3 + (int)(FWD_STRIDE_LENGTH) - (int)(FWD_LIFT_HEIGHT * (1.0 - ratio));
      // 右后(内侧)放腿位置 = 正常位置 - STRIDE_LENGTH * turnFactor
      int d9 = INIT_D9 - (int)(FWD_STRIDE_LENGTH * turnFactor) + (int)(FWD_LIFT_HEIGHT * (1.0 - ratio));
      int d5 = INIT_D5 + (int)(FWD_BODY_DROP * (1.0 - ratio)); // 右前抬腿
      int d6 = INIT_D6 - (int)(FWD_BODY_DROP * (1.0 - ratio)); // 左后抬腿
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }

    // 阶段4: 抬起另一对角腿 (右前+左后)
    Serial.print(F("4/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      int d5 = INIT_D5 + (int)(FWD_LIFT_HEIGHT * ratio);      // 右前抬腿
      int d6 = INIT_D6 - (int)(FWD_LIFT_HEIGHT * ratio);      // 左后抬腿
      int d3 = INIT_D3 - (int)(FWD_BODY_DROP * ratio);        // 左前下压
      int d9 = INIT_D9 + (int)(FWD_BODY_DROP * ratio);        // 右后下压
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }

    // 阶段5: 移动新抬起的腿 (右前+左后) - 右转关键: 内侧腿步幅减小
    Serial.print(F("5/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 右前(内侧)步幅 = 正常步幅 * turnFactor
      int d5 = INIT_D5 + FWD_LIFT_HEIGHT - (int)((FWD_STRIDE_LENGTH * turnFactor + FWD_LIFT_HEIGHT) * ratio);
      // 左后(外侧)保持正常步幅
      int d6 = INIT_D6 - FWD_LIFT_HEIGHT + (int)((FWD_STRIDE_LENGTH + FWD_LIFT_HEIGHT) * ratio);
      
      servo_D5.write(d5);
      servo_D6.write(d6);
      
      delay(stepDelay / WALK_CYCLES);
    }

    // 阶段6: 放下新抬起的腿
    Serial.println(F("6/6..."));
    for (int i = 0; i <= WALK_CYCLES; i++) {
      float ratio = (float)i / WALK_CYCLES;
      // 右前(内侧)放腿位置 = 正常位置 - STRIDE_LENGTH * turnFactor
      int d5 = INIT_D5 - (int)(FWD_STRIDE_LENGTH * turnFactor) + (int)(FWD_LIFT_HEIGHT * (1.0 - ratio));
      // 左后(外侧)放腿位置 = 正常位置 + STRIDE_LENGTH
      int d6 = INIT_D6 + (int)(FWD_STRIDE_LENGTH) - (int)(FWD_LIFT_HEIGHT * (1.0 - ratio));
      int d3 = INIT_D3 - (int)(FWD_BODY_DROP * (1.0 - ratio)); // 左前微调
      int d9 = INIT_D9 + (int)(FWD_BODY_DROP * (1.0 - ratio)); // 右后微调
      
      servo_D3.write(d3);
      servo_D5.write(d5);
      servo_D6.write(d6);
      servo_D9.write(d9);
      
      delay(stepDelay / WALK_CYCLES);
    }
  }
  stand();
}

// 显示表情函数 - 修改版：只保留动画，移除所有文字
void displayEmotion(Emotion emotion) {
  currentEmotion = emotion; // 更新当前表情
  u8g2.clearBuffer();
  
  // 表情图形中心位置
  const int centerX = 64;
  const int centerY = 32;

  // 表情细节
  switch(emotion) {
    case NEUTRAL:
      if (!eyesClosed) {
        // 睁眼
        u8g2.drawHLine(centerX - 20, centerY - 5, 8);
        u8g2.drawHLine(centerX + 12, centerY - 5, 8);
        // 中性嘴
        u8g2.drawHLine(centerX - 15, centerY + 10, 30);
      } else {
        // 闭眼
        u8g2.drawHLine(centerX - 22, centerY - 5, 14);
        u8g2.drawHLine(centerX + 8, centerY - 5, 14);
        // 中性嘴
        u8g2.drawHLine(centerX - 15, centerY + 10, 30);
      }
      break;
      
    case HAPPY:
      if (!eyesClosed) {
        // 睁眼
        u8g2.drawDisc(centerX - 15, centerY - 5, 4);
        u8g2.drawDisc(centerX + 15, centerY - 5, 4);
      }
      // 微笑嘴
      u8g2.drawCircle(centerX, centerY + 5, 12, U8G2_DRAW_LOWER_LEFT|U8G2_DRAW_LOWER_RIGHT);
      break;
      
    case EXCITED:
      // 大眼睛
      u8g2.drawDisc(centerX - 15, centerY - 10, 6);
      u8g2.drawDisc(centerX + 15, centerY - 10, 6);
      // O型嘴
      u8g2.drawCircle(centerX, centerY + 10, 8, U8G2_DRAW_ALL);
      break;
      
    case SLEEPY:
      // Zzz眼睛
      u8g2.drawHLine(centerX - 22, centerY - 5, 14);
      u8g2.drawHLine(centerX + 8, centerY - 5, 14);
      // Zzz符号
      u8g2.drawLine(centerX - 15, centerY + 5, centerX - 5, centerY + 15);
      u8g2.drawLine(centerX - 5, centerY + 15, centerX + 5, centerY + 5);
      u8g2.drawLine(centerX + 5, centerY + 5, centerX + 15, centerY + 15);
      break;
      
    case CONFUSED:
      // 困惑眼睛
      u8g2.drawLine(centerX - 20, centerY - 10, centerX - 10, centerY);
      u8g2.drawLine(centerX + 10, centerY - 10, centerX + 20, centerY);
      // 困惑嘴
      u8g2.drawLine(centerX - 15, centerY + 10, centerX, centerY + 5);
      u8g2.drawLine(centerX, centerY + 5, centerX + 15, centerY + 10);
      break;
      
    case SURPRISED:
      // 大眼睛
      u8g2.drawDisc(centerX - 15, centerY - 10, 6);
      u8g2.drawDisc(centerX + 15, centerY - 10, 6);
      // O型嘴
      u8g2.drawCircle(centerX, centerY + 10, 10, U8G2_DRAW_ALL);
      break;
  }
  
  u8g2.sendBuffer();
}

// 眨眼动画
void blinkAnimation() {
  if (currentEmotion != EXCITED && currentEmotion != CONFUSED) {
    eyesClosed = true;
    displayEmotion(currentEmotion);
    delay(150);
    eyesClosed = false;
    displayEmotion(currentEmotion);
  }
}